#include "RigidBody.h"

RigidBody::RigidBody(void)
{
	transform = State();
	previous = State();
	pi = 3.1415;
}

RigidBody::~RigidBody(void)
{
}

void RigidBody::Update(double t, double dt)
{
	previous = transform;
	Integrate(transform, t, dt);
}

// Evaluates the derivative of a State using the passed Derivative, a time value and a delta time value
Derivative RigidBody::Evaluate(State &initial, double t, double dt, Derivative &derivative)
{
	initial.position += derivative.velocity * dt;
	initial.momentum += derivative.force * dt;
	initial.orientation += derivative.spin * dt;
	initial.angularMomentum += derivative.torque * dt;

	Derivative output;
	output.velocity = initial.velocity;
	output.spin = initial.spin;
	output.force = initial.force + initial.velocity * initial.fDamp;
	output.torque = initial.torque + initial.angularVelocity * initial.tDamp;

	return output;
}

// Integrate a State object using RK4
void RigidBody::Integrate(State &state, double t, double dt)
{
	Derivative a = Evaluate(state, t, dt, Derivative());
	Derivative b = Evaluate(state, t, dt * 0.5, a);
	Derivative c = Evaluate(state, t, dt * 0.5, b);
	Derivative d = Evaluate(state, t, dt, c);

	state.position += (a.velocity + (b.velocity + c.velocity) * 2.0 + d.velocity) * 1.0/6.0 * dt;
	state.momentum += (a.force + (b.force + c.force) * 2.0 + d.force) * 1.0/6.0 * dt;
	state.orientation += (a.spin + (b.spin + c.spin) * 2.0 + d.spin) * 1.0/6.0 * dt;
	state.angularMomentum += (a.torque + (b.torque + c.torque) * 2.0 + d.torque) * 1.0/6.0 * dt;

	state.Recalculate();
}

void RigidBody::Render()
{
}

void RigidBody::ClearForces()
{
	transform.force = vector3();
	transform.torque = vector3();
}